/**
   Copyright 2008 Kirill Ignatiev

    This file is part of kerr-wavefronts.

    kerr-wavefronts is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    kerr-wavefronts is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with kerr-wavefronts.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_sf_lambert.h>
#include <gsl/gsl_odeiv.h>
#include <math.h>
#include "solvegeods.h"
#include <cassert>

double schw_x_of_r(double r) {
  return r-1 + log(r-1);
}
double schw_r_of_x(double x) {
  static gsl_sf_result r;
  gsl_sf_lambert_W0_e(exp(x), &r);
  return 1.0 + r.val;
}

int schw_f(double t, const double* y, double* dydt, void* _l) {
  double l = *(double*)_l;
  double x = y[0], rdot = y[2];
  double r = schw_r_of_x(x);
  double rho = 1-1/r;
  dydt[0] = rdot;
  dydt[1] = l*rho/(r*r);
  dydt[2] = (rdot*rdot + 2.0*l*l*rho*rho/r - 1.0)/(2.0*r*r*rho);
  return GSL_SUCCESS;
}

int schw_jac(double t, const double* y,
             double* dfdy, double* dfdt, double* _l) {
  double l = *_l;
  double x=y[0], rdot=y[2];
  double r = schw_r_of_x(x);
  double rho = 1-1/r;
  dfdy[3] = -2*l*rho/(r*r*r);
  dfdy[6] = (rdot*rdot+2*l*l*rho*rho/r-1)/(r*r*r) -l*l*rho*rho/(r*r*r*r);
  return GSL_SUCCESS;
}

double kerr_x_of_r(const kerr_param& param, double r) {
  double r1 = param.rplus;
  return r - r1 + r1*log((r-r1)/r1);
}
double kerr_r_of_x(const kerr_param& p, double x) {
  double r1 = p.rplus;
  static gsl_sf_result alpha;
  gsl_sf_lambert_W0_e(exp(x/r1), &alpha);
  return r1*(1 + alpha.val);
}

int kerr_f(double t, const double* y, double* dydt, void* params) {
  // y = (x, phi, p_r)
  // dydt = (xdot, phidot, p_r')
  if (!isfinite(y[0]) || !isfinite(y[1]) || !isfinite(y[2])) {
    GSL_ERROR("One of kerr derivatives is not finite.", GSL_ESING);
    return GSL_ESING;
  }

  kerr_param p = *(kerr_param*)params;
  static const double M = 0.5;
  double rplus = p.rplus, rminus = p.rminus, a = p.a, l = p.l, Q = p.Q;
  double r = kerr_r_of_x(p, y[0]);
  double Delta = r*r - r + Q*Q + a*a;
  double Delta_alpha = r*r+a*a-a*l;
  double beta = l-a;

  double Delta_r2_tdot = (r*r+a*a)*(r*r+a*a-a*l) + a*Delta*(l-a);
  double Delta_r2_phidot = a*(r*r+a*a-a*l)+(l-a)*Delta;

  dydt[0] = r*r*r*(r-rminus)*y[2]/Delta_r2_tdot;
  dydt[1] = Delta_r2_phidot / Delta_r2_tdot;

  double f1 = Delta - r*(r-0.5);
  double f2 = (r-0.5)/(r*r)-Delta/(r*r*r);
  double rdot = Delta/(r*r)*y[2];
  double Delta_prdot = 1/(r*r) *
    (-(r-0.5)*Delta_alpha*Delta_alpha/(r*r)
     +Delta_alpha*Delta_alpha*Delta/(r*r*r)
     +beta/(r*r*r)*(2*Delta_r2_phidot-Delta*beta)*Delta
     -rdot*rdot*r*(Delta-r*(r-0.5)));
  dydt[2] = r*r * Delta_prdot / Delta_r2_tdot;

  if (!isfinite(dydt[0]) || !isfinite(dydt[1]) || !isfinite(dydt[2])) {
    GSL_ERROR("One of kerr derivatives is not finite.", GSL_ESING);
    //return GSL_ESING;
  }
  return GSL_SUCCESS;
}


